// $Id:.c 177 2013-06-24 23:09:27Z alan.watson.f $

#define _POSIX_C_SOURCE 200112L

#include <limits.h>
#include <float.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#include "oaxaca-config.h"
#include "oaxaca.h"
#include "array.h"
#include "minmax.h"
#include "rfits.h"
#include "spots.h"
#include "wpng.h"

////////////////////////////////////////////////////////////////////////////////

static int
short_cmp(const void *ap, const void *bp)
{
  short a = * (short *) ap;
  short b = * (short *) bp;
  if (a < b)
    return -1;
  else if (a > b)
    return +1;
  else
    return 0;
}

static short
medianfilter_3x3(array_short_t *a, int iy, int ix)
{
  short z[] = {
    a->z[iy - 1][ix - 1],
    a->z[iy - 1][ix + 0],
    a->z[iy - 1][ix + 1],
    a->z[iy + 0][ix - 1],
    a->z[iy + 0][ix + 0],
    a->z[iy + 0][ix + 1],
    a->z[iy + 1][ix - 1],
    a->z[iy + 1][ix + 0],
    a->z[iy + 1][ix + 1],
  };
  size_t n = sizeof(z) / sizeof(*z);
  qsort(z, sizeof(z) / sizeof(*z), sizeof(*z), short_cmp);
  return z[n / 2];
}

////////////////////////////////////////////////////////////////////////////////

static array_short_t *
read_latest(void)
{
  oaxaca_info("reading latest image.");
  FILE *fp = fopen("latest.fits", "rb");
  if (fp == NULL) {
    oaxaca_info("unable to open latest image.");
    return NULL;
  }
  array_short_t *a = oaxaca_rfits(fp);
  fclose(fp);
  if (a == NULL) {
    oaxaca_info("unable to read latest image.");
    return NULL;
  }
  return a;
}

void
oaxaca_spots_find(array_short_t *a, int ns, double *s, int searchradius, int refinementradius)
{
  oaxaca_info("finding ss.");
  
  int free_a = 0;
  if (a == NULL) {
    free_a = 1;
    a = read_latest();
    if (a == NULL)
      return;
  }
  
  oaxaca_info("filtering image with 3x3 median filter.");
  array_short_t *b = alloc_array_short(a->ny, a->nx);
  for (int iy = 0; iy < b->ny; ++iy)
    for (int ix = 0; ix < b->nx; ++ix)
      if (iy == 0 || iy == b->ny - 1 || ix == 0 || ix == b->nx - 1)
        b->z[iy][ix] = 0;
      else
        b->z[iy][ix] = medianfilter_3x3(a, iy, ix);
    
  oaxaca_info("finding spots.");
  for (int i = 0; i < ns / 2; ++i) {
    // Find maximum.
    int max_ix = 0;
    int max_iy = 0;
    short max_z = SHRT_MIN;
    for (int iy = 0; iy < b->ny; ++iy)
      for (int ix = 0; ix < b->nx; ++ix)
        if (b->z[iy][ix] > max_z) {
          max_z = b->z[iy][ix];
          max_ix = ix;
          max_iy = iy;
        }
    oaxaca_info("found s %d with maximum of %d at (%d,%d).", i, max_z, max_ix, max_iy);
    s[2 * i + 0] = max_ix;
    s[2 * i + 1] = max_iy;
    for (int iy = 0; iy < a->ny; ++iy)
      for (int ix = 0; ix < a->nx; ++ix) {
        int dx = ix - max_ix;
        int dy = iy - max_iy;
        if (dx * dx + dy * dy <= searchradius * searchradius)
          b->z[iy][ix] = SHRT_MIN;
      }
  }
  
  oaxaca_info("refining spots.");
  oaxaca_spots_refine(a, ns, s, searchradius, refinementradius, s);
  for (int i = 0; i < ns / 2; ++i) {
    oaxaca_info("spot %d at (%.2f,%.2f).", i, s[2 * i + 0], s[2 * i + 1]);
  }

  oaxaca_wpng_write_file(a, ns, s, searchradius, refinementradius);
  
  if (free_a)
    free_array_short(a);
  free_array_short(b);
  
  oaxaca_info("finished finding spots.");
  
  return;
}

////////////////////////////////////////////////////////////////////////////////

void
oaxaca_spots_refine(array_short_t *a, int ns, double *r, int searchradius, int refinementradius, double *s)
{
  int free_a = 0;
  if (a == NULL) {
    free_a = 1;
    a = read_latest();
    if (a == NULL)
      return;
  }
  
  int nx = a->nx;
  int ny = a->ny;
  
  for (int i = 0; i < ns / 2; ++i) {

    int rx = (int) (r[2 * i + 0] + 0.5);
    int ry = (int) (r[2 * i + 1] + 0.5);

    // Find maximum in (2*searchradius+1) region around the reference spot.

    int ixmax;
    int iymax;

    {
      int sx = imax(rx - searchradius, 0);
      int sy = imax(ry - searchradius, 0);
      int ex = imin(rx + searchradius, nx - 1);
      int ey = imin(ry + searchradius, ny - 1);
  
      ixmax = rx;
      iymax = ry;
      double zmax = a->z[iymax][ixmax];
      for (int iy = sy; iy <= ey; ++iy) {
        for (int ix = sx; ix <= ex; ++ix) {
          if ((ix - rx) * (ix - rx) + (iy - ry) * (iy - ry) <= searchradius * searchradius) {
            if (a->z[iy][ix] > zmax) {
              iymax = iy;
              ixmax = ix;
              zmax = a->z[iy][ix];
            }
          }
        }
      }
    }


    // Now find the centroid in a (2*refinementradius+1) region around the peak.
    
    double xc;
    double yc;
    
    {

      int sx = imax(ixmax - refinementradius, 0);
      int sy = imax(iymax - refinementradius, 0);
      int ex = imin(ixmax + refinementradius, nx - 1);
      int ey = imin(iymax + refinementradius, ny - 1);
  
      long w = 0;
      long wx = 0;
      long wy = 0;
      for (int iy = sy; iy <= ey; ++iy) {
        for (int ix = sx; ix <= ex; ++ix) {
          if ((ix - rx) * (ix - rx) + (iy - ry) * (iy - ry) <= refinementradius * refinementradius) {
            w  += a->z[iy][ix];
            wx += a->z[iy][ix] * ix;
            wy += a->z[iy][ix] * iy;
          }
        }
      }
    
      xc = (double) wx / (double) w;
      yc = (double) wy / (double) w;
    }
    
    if (xc > ixmax + refinementradius ||
        xc < ixmax - refinementradius ||
        xc != xc)
      xc = ixmax;
    if (yc > iymax + refinementradius ||
        yc < iymax - refinementradius ||
        yc != yc)
      yc = iymax;
    
    s[2 * i + 0] = xc;
    s[2 * i + 1] = yc;

  }

  if (free_a)
    free_array_short(a);

}
